#ifndef KD_DEBUG_TRACER_H
#define KD_DEBUG_TRACER_H

#include "../RayTraceGPU/MeshGeometry.h"

namespace RayTracePro
{
#ifndef DBG_KD_TRAVERSE_STATS
#define DBG_KD_TRAVERSE_STATS 1
#endif
	template<bool pred>
	class MeshTracer
	{
	public:
		MeshGeometry * mesh;
		inline bool Trace(DifferentialGeometry & inter, const MeshFace & face, int faceId, const Ray & ray, const RayDifferential & diffIn, float & t) const
		{
			const int mod3[] = {0,1,2,0,1};
			int u = mod3[face.ProjectionAxis + 1];
			int v = mod3[face.ProjectionAxis + 2];
			float invNdotD = 1.0f / (face.PlaneU * ray.Direction[u] + face.PlaneV * ray.Direction[v] + ray.Direction[face.ProjectionAxis]);
			float tplane = -(face.PlaneU * ray.Origin[u] + face.PlaneV * ray.Origin[v] + ray.Origin[face.ProjectionAxis] + face.PlaneD) * invNdotD;
			if (tplane < ray.tMin-Epsilon || tplane > ray.tMax+Epsilon)
				return false;
			float hitU = ray.Origin[u] + ray.Direction[u] * tplane;
			float hitV = ray.Origin[v] + ray.Direction[v] * tplane;
			
			float beta = face.K_beta_u * hitU + face.K_beta_v * hitV + face.K_beta_d;
			if (beta < -Epsilon)
				return false;
			float gamma = face.K_gamma_u * hitU + face.K_gamma_v * hitV + face.K_gamma_d;
			if (gamma < -Epsilon)
				return false;
			if (beta + gamma > 1.0f + Epsilon)
				return false;
			if (pred)
				return true;
			inter.Position[u] = hitU;
			inter.Position[v] = hitV;
			inter.Position[face.ProjectionAxis] = ray.Origin[face.ProjectionAxis] + ray.Direction[face.ProjectionAxis] * tplane;
			float alpha = 1.0f - beta - gamma;
			Vec3 tmp;
			Vec3::Scale(inter.Normal, mesh->Normals[face.Normal1], alpha);
			Vec3::Scale(tmp, mesh->Normals[face.Normal2], beta);
			inter.Normal += tmp;
			Vec3::Scale(tmp, mesh->Normals[face.Normal3], gamma);
			inter.Normal += tmp;

			Vec2 tmpVec2;
			if (face.TexCoord1 != -1)
			{
				inter.UV.x = mesh->TexCoords[face.TexCoord1].x * alpha;
				inter.UV.y = mesh->TexCoords[face.TexCoord1].y * alpha;
				inter.UV.x += mesh->TexCoords[face.TexCoord2].x * beta;
				inter.UV.y += mesh->TexCoords[face.TexCoord2].y * beta;
				inter.UV.x += mesh->TexCoords[face.TexCoord3].x * gamma;
				inter.UV.y += mesh->TexCoords[face.TexCoord3].y * gamma;
			}
			inter.dNdxContinuation = faceId;
			t = tplane;
			return true;
		}
	};

	template<typename T, typename Tracer, bool pred, bool useMailbox>
	bool TraverseKdTreeStacklessDbg(RayTraceSystem * system, const Tracer & tracer, 
		DifferentialGeometry & rs, KdTree<T> & tree, const Ray & ray, const RayDifferential & rayDiff, float & t,
		Array<int, true> & traversedNodes)
	{
		float sceneMin, sceneMax;
		if (!RayBBoxIntersection(tree.Bounds, ray, sceneMin, sceneMax))
			return false;
		Ray traceRay = ray;
		traceRay.tMax = sceneMax;
		traceRay.tMin = sceneMin;
		rs.Normal = traceRay.Direction;
		rs.Position.SetZero();
		bool intersected = false;
		float tMin = sceneMin;
		float tMax = sceneMax;
		KdNode * node = tree.Nodes.Buffer();
		while (tMin < tMax)
		{
			
			while (!node->GetIsLeaf())
			{
				int axis = (int)(node->GetSplitDimension());
				float tplane;
				tplane = (node->SplitPlane - ray.Origin[axis]) * ray.ReciprocalDirection[axis];
				KdNode* node1, *node2;
				float rayOrigin = ray.Origin[axis];
				if (rayOrigin < node->SplitPlane)
				{
					node1 = node + 1;
					node2 = (KdNode*)((char*)tree.Nodes.Buffer() + node->GetOffset());
				}
				else
				{
					node1 = (KdNode*)((char*)tree.Nodes.Buffer() + node->GetOffset());
					node2 = node + 1;
				}
				if (tplane >= tMax || tplane < 0)
				{
					node = node1;
				}
				else if (tplane <= tMin)
				{
					node = node2;
				}
				else
				{
					node = node1;
				}
				traversedNodes.Add(node-tree.Nodes.Buffer());
			}
			DifferentialGeometry inter;
			int count = node->GetElementCount();
			KdLeafHeader * leaf = (KdLeafHeader*)((char*)tree.Leaves.Buffer() + node->LeafPtr);
			int* elemIds = (int*)(leaf+1);
			for (int i = 0; i < count; i++)
			{
				T*curElem = tree.Elements.Buffer() + elemIds[i];
				float it = FLT_MAX;
				if (tracer.Trace(inter, *curElem, elemIds[i], traceRay, rayDiff, it))
				{
					if (pred)
						return true;
					// bias t
					it += system->GeometryBias*Vec3::Dot(inter.Normal, ray.Direction);
					if (it<tMax)
					{
						tMax = it;
						rs = inter;
						t = it;
						intersected = true;
					}
				}
			}
			// Locate next node
			int face;
			RayBBoxExit(leaf->Bounds, ray, tMin, tMin, face);
			//tMin -= Epsilon;
			int nextPtr = leaf->Ropes[face];
			if (nextPtr == -1)
				break;
			node = (KdNode*)((char*)tree.Nodes.Buffer() + nextPtr);
			traversedNodes.Add(node-tree.Nodes.Buffer());
			if (traversedNodes.Count() > 1000)
			{
				printf("trace dead loop\n");
				break;
			}
		}
		
		return intersected;
	}
}

#endif